#include "mex.h"
#include <ctype.h> 
#include <math.h>  
#include <stdarg.h>
#include <stdio.h> 
#include <stdlib.h>
#include <string.h>

/* Degrees to radians conversion */
#define Pi			3.14159265358979323846264338327950288419716939937510582
#define DEGtoRAD	0.01745329251994329576923690768488612713442871888541725
#define	RADtoDEG	57.2957795130823208767981548141051703324054724665643215
#define G			9.81
#define _NAN        9.99E+305

void     define(const double *Torque, const double *Q, const double *U, const double *Theta);
void	 evaluate(void);
void     output(double dfdu[], double dfdx[]);

double   BETA,C1,C2,C3,C4,C5,C6,L1,L2,LN,M1,M2,M3,R,T1,T2,T3,T4,T5,T6,V1,V2,V3,
  V4,V5,V6,Q1,Q2,Q3,Q4,Q5,Q6,U1,U2,U3,U4,U5,U6;
double   TF1,TF2,TF3,TF4,TF5,TF6;
double   z[540],DFDU[12][6],DFDX[12][12];


/* ................................ MAIN ............................. */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	int ii, jj, n; int m = 0;
	double *Torque, *Q, *U, *Theta;
	double *dfdu, *dfdx;

    /* Assign pointers to each input and output. */
	Torque = mxGetPr(prhs[0]);
	Q  = mxGetPr(prhs[1]);
	U = mxGetPr(prhs[2]);
	Theta = mxGetPr(prhs[3]);
    if (mxGetM(prhs[0]) != 6)
    {
        mexErrMsgTxt("Not enough torques specified.");
    }

    if (mxGetM(prhs[1]) != 6)
    {
        mexErrMsgTxt("Not enough Qs specified.");
    }

    if (mxGetM(prhs[2]) != 6)
    {
        mexErrMsgTxt("Not enough Qps specified.");
    }

    if (mxGetM(prhs[3]) != 20)
    {
        mexErrMsgTxt("Not enough arguments specified for theta vector.");
    }
	
	/* Evaluate output quantities */
	define(Torque,Q,U,Theta);
	evaluate();
	
   	/* Create matrix for the return argument. */
	plhs[0] = mxCreateDoubleMatrix(12, 6, mxREAL);
	dfdu = mxGetPr(plhs[0]);
	
	plhs[1] = mxCreateDoubleMatrix(12,12, mxREAL);
	dfdx = mxGetPr(plhs[1]);

    output(dfdu, dfdx);
}

/* ............................. DEFINITIONS ........................... */

void define(const double *Torque, const double *Q, const double *U, const double *Theta)
{
	// define joint torques from input
	T1 = Torque[0];
	T2 = Torque[1];
	T3 = Torque[2];
	T4 = Torque[3];
	T5 = Torque[4];
	T6 = Torque[5];

	// define joint angles from input
	Q1 = Q[0];
	Q2 = Q[1];
	Q3 = Q[2];
	Q4 = Q[3];
	Q5 = Q[4];
	Q6 = Q[5];
	
	// define joint velocities from input
	U1 = U[0];
	U2 = U[1];
	U3 = U[2];
	U4 = U[3];
	U5 = U[4];
	U6 = U[5];
	
	// define robot parameters from theta
    LN = Theta[0];
    L1 = Theta[1];
    L2 = Theta[2];
    R = Theta[3];
    M1 = Theta[4];
    M2 = Theta[5];
    M3 = Theta[6];
    C1 = Theta[7];
    C2 = Theta[8];
    C3 = Theta[9];
    C4 = Theta[10];
    C5 = Theta[11];
    C6 = Theta[12];
    V1 = Theta[13];
    V2 = Theta[14];
    V3 = Theta[15];
    V4 = Theta[16];
    V5 = Theta[17];
    V6 = Theta[18];
    BETA = Theta[19];
}

/* ................................ OUTPUT ............................. */

void output(double dfdu[], double dfdx[])
{
	int r = 0, c = 0;
	for (r=0; r < 12; ++r)
	{
		for (c = 0; c < 6; ++c) {
			dfdu[c*12 + r] = DFDU[r][c];
			dfdx[c*12 + r] = DFDX[r][c];
		}
		
		for (c = 6; c < 12; ++c) {
			dfdx[c*12 + r] = DFDX[r][c];
		}
	}
}

/* ................................ EQNS .............................. */

void evaluate(void)
{
/* Evaluate constants */
  z[1] = M1*pow(L1,2);
  z[2] = M2*pow(L2,2);
  z[3] = M3*pow(R,2);
  TF1 = V1*U1 + C1*tanh(BETA*U1);
  TF2 = V2*U2 + C2*tanh(BETA*U2);
  TF3 = V3*U3 + C3*tanh(BETA*U3);
  TF4 = V4*U4 + C4*tanh(BETA*U4);
  TF5 = V5*U5 + C5*tanh(BETA*U5);
  TF6 = V6*U6 + C6*tanh(BETA*U6);
  z[4] = cos(Q1);
  z[5] = sin(Q1);
  z[6] = cos(Q2);
  z[7] = sin(Q2);
  z[8] = cos(Q3);
  z[9] = sin(Q3);
  z[10] = cos(Q4);
  z[11] = sin(Q4);
  z[12] = cos(Q5);
  z[13] = sin(Q5);
  z[14] = cos(Q6);
  z[15] = sin(Q6);
  z[16] = pow(z[4],2) + pow(z[5],2);
  z[17] = pow(z[6],2) + pow(z[7],2);
  z[18] = pow(z[8],2) + pow(z[9],2);
  z[19] = pow(z[10],2) + pow(z[11],2);
  z[20] = pow(z[12],2) + pow(z[13],2);
  z[21] = pow(z[14],2) + pow(z[15],2);
  z[22] = z[16]*U1;
  z[23] = z[17]*U2;
  z[24] = z[18]*U3;
  z[28] = z[19]*U4;
  z[29] = z[20]*U5;
  z[30] = z[21]*U6;
  z[34] = LN*z[16];
  z[35] = L1*z[16];
  z[36] = L1*z[17];
  z[37] = L2*z[16];
  z[38] = L2*z[17];
  z[39] = z[4]*z[34];
  z[40] = z[5]*z[34];
  z[41] = z[39] - z[35];
  z[42] = z[6]*z[41] - z[7]*z[40];
  z[43] = z[6]*z[36];
  z[44] = z[7]*z[36];
  z[45] = -z[6]*z[40] - z[7]*z[41];
  z[46] = L2*z[18];
  z[47] = z[42] - z[37];
  z[48] = -z[38] - z[43];
  z[49] = LN*z[19];
  z[50] = L1*z[19];
  z[51] = L1*z[20];
  z[52] = L2*z[19];
  z[53] = L2*z[20];
  z[54] = z[10]*z[49];
  z[55] = z[11]*z[49];
  z[56] = z[54] - z[50];
  z[57] = z[12]*z[56] - z[13]*z[55];
  z[58] = z[12]*z[51];
  z[59] = z[13]*z[51];
  z[60] = -z[12]*z[55] - z[13]*z[56];
  z[61] = L2*z[21];
  z[62] = z[57] - z[52];
  z[63] = -z[53] - z[58];
  z[64] = z[34]*U1*z[22];
  z[65] = z[35]*U1*z[22];
  z[66] = (z[35]*U1+z[36]*U2)*(z[22]+z[23]);
  z[67] = (z[37]*U1+z[38]*U2)*(z[22]+z[23]);
  z[68] = z[5]*z[64];
  z[69] = z[4]*z[64];
  z[70] = z[69] - (z[35]*U1+z[36]*U2)*(z[22]+z[23]);
  z[71] = z[6]*z[68] + z[7]*z[70];
  z[72] = z[6]*z[70] - z[7]*z[68];
  z[73] = z[72] - (z[37]*U1+z[38]*U2+z[46]*U3)*(z[22]+z[23]+z[24]);
  z[74] = z[49]*U4*z[28];
  z[75] = z[50]*U4*z[28];
  z[76] = (z[50]*U4+z[51]*U5)*(z[28]+z[29]);
  z[77] = (z[52]*U4+z[53]*U5)*(z[28]+z[29]);
  z[81] = L1*z[21];
  z[82] = (z[50]*U4+z[51]*U5+z[81]*U6)*(z[28]+z[29]+z[30]);
  z[83] = (z[52]*U4+z[53]*U5+z[61]*U6)*(z[28]+z[29]+z[30]);
  z[84] = T1 - TF1;
  z[85] = T2 - TF2;
  z[86] = T3 - TF3;
  z[87] = T4 - TF4;
  z[88] = T5 - TF5;
  z[89] = T6 - TF6;
  z[90] = z[16]*z[84];
  z[91] = z[17]*z[85];
  z[92] = z[18]*z[86];
  z[93] = z[19]*z[87];
  z[94] = z[20]*z[88];
  z[95] = z[21]*z[89];
  z[96] = z[1]*z[16];
  z[97] = z[1]*z[19];
  z[100] = z[2]*z[16];
  z[101] = z[2]*z[17];
  z[104] = z[2]*z[19];
  z[105] = z[2]*z[20];
  z[109] = z[3]*z[16];
  z[110] = z[3]*z[17];
  z[111] = z[3]*z[18];
  z[115] = z[3]*z[19];
  z[116] = z[3]*z[20];
  z[117] = z[3]*z[21];
  z[118] = z[4]*z[6] - z[5]*z[7];
  z[119] = z[4]*z[7] + z[5]*z[6];
  z[120] = -z[4]*z[7] - z[5]*z[6];
  z[121] = z[11]*z[13] - z[10]*z[12];
  z[122] = -z[10]*z[13] - z[11]*z[12];
  z[123] = z[10]*z[13] + z[11]*z[12];
  z[124] = 0.08333333333333333*z[16]*z[96] + 0.08333333333333333*z[16]*z[100] + 
  0.4*z[16]*z[109] + M3*(pow(z[45],2)+pow(z[47],2)) + 0.25*M1*(pow(z[35],2)+4*
  pow(z[34],2)-4*z[4]*z[34]*z[35]) + 0.25*M2*(pow(z[37],2)+4*pow(z[34],2)+4*
  pow(z[35],2)+4*z[6]*z[35]*z[37]-8*z[4]*z[34]*z[35]-4*z[34]*z[37]*z[118]);
  z[125] = 0.08333333333333333*z[16]*z[101] + 0.4*z[16]*z[110] + M3*(z[44]*
  z[45]+z[47]*z[48]) + 0.25*M2*(z[37]*z[38]+4*z[35]*z[36]+2*z[6]*z[35]*z[38]+
  2*z[6]*z[36]*z[37]-4*z[4]*z[34]*z[36]-2*z[34]*z[38]*z[118]);
  z[126] = 0.4*z[16]*z[111] - M3*z[46]*z[47];
  z[127] = M3*(z[45]*z[73]+z[47]*z[71]) + 0.5*M1*z[5]*(z[34]*z[65]-z[35]*
  z[64]) + 0.5*M2*(z[7]*z[37]*z[66]+2*z[5]*z[34]*z[66]-2*z[5]*z[35]*z[64]-
  z[7]*z[35]*z[67]-z[34]*z[120]*z[67]-z[37]*z[119]*z[64]);
  z[128] = 0.08333333333333333*z[17]*z[100] + 0.4*z[17]*z[109] + M3*(z[44]*
  z[45]+z[47]*z[48]) + 0.25*M2*(z[37]*z[38]+4*z[35]*z[36]+2*z[6]*z[35]*z[38]+
  2*z[6]*z[36]*z[37]-4*z[4]*z[34]*z[36]-2*z[34]*z[38]*z[118]);
  z[129] = 0.08333333333333333*z[17]*z[101] + 0.4*z[17]*z[110] + M3*(pow(
  z[44],2)+pow(z[48],2)) + 0.25*M2*(pow(z[38],2)+4*pow(z[36],2)+4*z[6]*z[36]*
  z[38]);
  z[130] = 0.4*z[17]*z[111] - M3*z[46]*z[48];
  z[131] = M3*(z[44]*z[73]+z[48]*z[71]) + 0.5*M2*(z[7]*z[38]*z[66]-2*z[5]*
  z[36]*z[64]-z[7]*z[36]*z[67]-z[38]*z[119]*z[64]);
  z[132] = 0.4*z[18]*z[109] - M3*z[46]*z[47];
  z[133] = 0.4*z[18]*z[110] - M3*z[46]*z[48];
  z[134] = 0.4*z[18]*z[111] + M3*pow(z[46],2);
  z[135] = M3*z[46]*z[71];
  z[136] = 0.08333333333333333*z[19]*z[97] + 0.08333333333333333*z[19]*z[104] + 
  0.4*z[19]*z[115] + 0.25*M1*(pow(z[50],2)+4*pow(z[49],2)-4*z[10]*z[49]*z[50]) + 
  M3*(z[13]*z[50]*z[60]-z[52]*z[62]-z[12]*z[50]*z[62]-z[49]*z[60]*z[123]-
  z[49]*z[62]*z[121]) - 0.25*M2*(8*z[10]*z[49]*z[50]-4*pow(z[49],2)-4*pow(
  z[50],2)-pow(z[52],2)-4*z[12]*z[50]*z[52]-4*z[49]*z[52]*z[121]);
  z[137] = 0.08333333333333333*z[19]*z[105] + 0.4*z[19]*z[116] - M3*(z[53]*
  z[62]+z[12]*z[51]*z[62]-z[13]*z[51]*z[60]) - 0.25*M2*(4*z[10]*z[49]*z[51]-4*
  z[50]*z[51]-z[52]*z[53]-2*z[12]*z[50]*z[53]-2*z[12]*z[51]*z[52]-2*z[49]*
  z[53]*z[121]);
  z[138] = 0.4*z[19]*z[117] - M3*(z[61]*z[62]+z[12]*z[62]*z[81]-z[13]*z[60]*
  z[81]);
  z[139] = 0.5*M1*z[11]*(z[49]*z[75]-z[50]*z[74]) - M3*(z[60]*z[83]+z[12]*
  z[60]*z[82]+z[13]*z[62]*z[82]+z[60]*z[121]*z[74]+z[62]*z[122]*z[74]) - 0.5*
  M2*(z[13]*z[50]*z[77]+2*z[11]*z[50]*z[74]-2*z[11]*z[49]*z[76]-z[13]*z[52]*
  z[76]-z[49]*z[123]*z[77]-z[52]*z[122]*z[74]);
  z[140] = 0.08333333333333333*z[20]*z[104] + 0.4*z[20]*z[115] + M3*(z[13]*
  z[50]*z[59]-z[52]*z[63]-z[12]*z[50]*z[63]-z[49]*z[59]*z[123]-z[49]*z[63]*
  z[121]) - 0.25*M2*(4*z[10]*z[49]*z[51]-4*z[50]*z[51]-z[52]*z[53]-2*z[12]*
  z[50]*z[53]-2*z[12]*z[51]*z[52]-2*z[49]*z[53]*z[121]);
  z[141] = 0.08333333333333333*z[20]*z[105] + 0.4*z[20]*z[116] + 0.25*M2*(
  pow(z[53],2)+4*pow(z[51],2)+4*z[12]*z[51]*z[53]) - M3*(z[53]*z[63]+z[12]*
  z[51]*z[63]-z[13]*z[51]*z[59]);
  z[142] = 0.4*z[20]*z[117] - M3*(z[61]*z[63]+z[12]*z[63]*z[81]-z[13]*z[59]*
  z[81]);
  z[143] = -M3*(z[59]*z[83]+z[12]*z[59]*z[82]+z[13]*z[63]*z[82]+z[59]*z[121]*
  z[74]+z[63]*z[122]*z[74]) - 0.5*M2*(z[13]*z[51]*z[77]+2*z[11]*z[51]*z[74]-
  z[13]*z[53]*z[76]-z[53]*z[122]*z[74]);
  z[144] = 0.4*z[21]*z[115] + M3*z[61]*(z[52]+z[12]*z[50]+z[49]*z[121]);
  z[145] = 0.4*z[21]*z[116] + M3*z[61]*(z[53]+z[12]*z[51]);
  z[146] = 0.4*z[21]*z[117] + M3*z[61]*(z[61]+z[12]*z[81]);
  z[147] = M3*z[61]*(z[13]*z[82]+z[122]*z[74]);
  z[148] = z[90] - z[127];
  z[149] = z[91] - z[131];
  z[150] = z[92] + z[135];
  z[151] = z[93] - z[139];
  z[152] = z[94] - z[143];
  z[153] = z[95] - z[147];
  z[154] = z[128]/z[124];
  z[155] = z[125]*z[154] - z[129];
  z[156] = z[126]*z[154] - z[130];
  z[157] = z[154]*z[148] - z[149];
  z[158] = z[132]/z[124];
  z[159] = z[125]*z[158] - z[133];
  z[160] = z[126]*z[158] - z[134];
  z[161] = z[158]*z[148] - z[150];
  z[162] = z[159]/z[155];
  z[163] = z[160] - z[156]*z[162];
  z[164] = z[161] - z[162]*z[157];
  z[165] = z[164]/z[163];
  z[166] = (z[157]-z[156]*z[165])/z[155];
  z[168] = z[140]/z[136];
  z[169] = z[137]*z[168] - z[141];
  z[170] = z[138]*z[168] - z[142];
  z[171] = z[168]*z[151] - z[152];
  z[172] = z[144]/z[136];
  z[173] = z[137]*z[172] - z[145];
  z[174] = z[138]*z[172] - z[146];
  z[175] = z[172]*z[151] - z[153];
  z[176] = z[173]/z[169];
  z[177] = z[174] - z[170]*z[176];
  z[178] = z[175] - z[176]*z[171];
  z[179] = z[178]/z[177];
  z[180] = (z[171]-z[170]*z[179])/z[169];
  z[182] = z[16]*(z[158]-z[154]*z[162]);
  z[183] = (z[16]*z[154]-z[156]*z[182]/z[163])/z[155];
  z[184] = (z[16]-z[125]*z[183]-z[126]*z[182]/z[163])/z[124];
  z[185] = z[17]*(1+z[156]*z[162]/z[163])/z[155];
  z[186] = (z[125]*z[185]-z[17]*z[126]*z[162]/z[163])/z[124];
  z[187] = z[18]*z[156]/(z[155]*z[163]);
  z[188] = (z[125]*z[187]-z[18]*z[126]/z[163])/z[124];
  z[189] = z[19]*(z[172]-z[168]*z[176]);
  z[190] = (z[19]*z[168]-z[170]*z[189]/z[177])/z[169];
  z[191] = (z[19]-z[137]*z[190]-z[138]*z[189]/z[177])/z[136];
  z[192] = z[20]*(1+z[170]*z[176]/z[177])/z[169];
  z[193] = (z[137]*z[192]-z[20]*z[138]*z[176]/z[177])/z[136];
  z[194] = z[21]*z[170]/(z[169]*z[177]);
  z[195] = (z[137]*z[194]-z[21]*z[138]/z[177])/z[136];
  z[196] = z[34]*(z[4]*z[6]-z[5]*z[7]);
  z[197] = (z[4]*z[7]+z[5]*z[6])*z[64];
  z[198] = z[34]*(z[4]*z[7]+z[5]*z[6]);
  z[199] = (z[4]*z[6]-z[5]*z[7])*z[64];
  z[200] = z[5]*z[7] - z[4]*z[6];
  z[201] = 0.5*M1*z[4]*(z[34]*z[65]-z[35]*z[64]) + M3*(z[47]*z[199]-z[45]*
  z[197]-z[196]*z[73]-z[198]*z[71]) + 0.5*M2*(2*z[4]*z[34]*z[66]-2*z[4]*z[35]*
  z[64]-z[34]*z[200]*z[67]-z[37]*z[118]*z[64]);
  z[202] = 0.5*M2*z[34]*(2*z[5]*z[36]-z[38]*z[120]) - M3*(z[44]*z[196]+z[48]*
  z[198]);
  z[203] = M1*z[5]*z[34]*z[35] + M2*z[34]*(2*z[5]*z[35]-z[37]*z[120]) - 2*M3*(
  z[45]*z[196]+z[47]*z[198]);
  z[204] = (z[124]*z[202]-z[128]*z[203])/pow(z[124],2);
  z[205] = -M3*(z[44]*z[197]-z[48]*z[199]) - 0.5*M2*(z[38]*z[118]+2*z[4]*
  z[36])*z[64];
  z[206] = z[205] + z[204]*z[148] - z[154]*z[201];
  z[207] = z[126]*z[204] + M3*z[46]*z[154]*z[198];
  z[208] = (z[132]*z[203]-M3*z[46]*z[124]*z[198])/pow(z[124],2);
  z[209] = -z[158]*z[201] - z[208]*z[148] - M3*z[46]*z[199];
  z[210] = z[158]*z[202] - z[125]*z[208];
  z[211] = z[125]*z[204] + z[154]*z[202];
  z[212] = (z[155]*z[210]-z[159]*z[211])/pow(z[155],2);
  z[213] = z[209] - z[162]*z[206] - z[212]*z[157];
  z[214] = M3*z[46]*z[158]*z[198] - z[126]*z[208];
  z[215] = z[214] - z[156]*z[212] - z[162]*z[207];
  z[216] = (z[163]*z[213]-z[215]*z[164])/pow(z[163],2);
  z[217] = (z[211]*(z[157]-z[156]*z[165])-z[155]*(z[206]-z[156]*z[216]-z[207]*
  z[165]))/pow(z[155],2);
  z[218] = (z[203]*(z[148]-z[125]*z[166]-z[126]*z[165])-z[124]*(z[125]*z[217]-
  z[201]-z[126]*z[216]-z[202]*z[166]-M3*z[46]*z[198]*z[165]))/pow(z[124],2);
  z[219] = z[7]*z[40] - z[6]*z[41];
  z[220] = -z[6]*z[68] - z[7]*z[70];
  z[221] = M3*(z[45]*z[71]+z[45]*z[220]+z[47]*z[72]+z[219]*z[73]) + 0.5*M2*(
  z[6]*z[37]*z[66]-z[6]*z[35]*z[67]-z[34]*z[200]*z[67]-z[37]*z[118]*z[64]);
  z[222] = M3*(z[44]*z[219]+z[45]*z[48]+z[6]*z[36]*z[45]+z[7]*z[36]*z[47]) - 
  0.5*M2*(z[7]*z[35]*z[38]+z[7]*z[36]*z[37]+z[34]*z[38]*z[120]);
  z[223] = 2*M3*z[45]*(z[47]+z[219]) - M2*z[37]*(z[7]*z[35]+z[34]*z[120]);
  z[224] = (z[124]*z[222]-z[128]*z[223])/pow(z[124],2);
  z[225] = M3*(z[44]*z[220]+z[48]*z[72]+z[6]*z[36]*z[73]+z[7]*z[36]*z[71]) + 
  0.5*M2*(z[6]*z[38]*z[66]-z[6]*z[36]*z[67]-z[38]*z[118]*z[64]);
  z[226] = z[225] + z[224]*z[148] - z[154]*z[221];
  z[227] = M3*z[7]*z[36]*z[46];
  z[228] = z[227] + z[126]*z[224] - M3*z[45]*z[46]*z[154];
  z[229] = (z[132]*z[223]+M3*z[45]*z[46]*z[124])/pow(z[124],2);
  z[230] = -z[158]*z[221] - z[229]*z[148] - M3*z[46]*z[72];
  z[231] = z[227] + z[158]*z[222] - z[125]*z[229];
  z[232] = z[36]*(M2*z[7]*z[38]-2*M3*(z[6]*z[44]+z[7]*z[48]));
  z[233] = z[232] + z[125]*z[224] + z[154]*z[222];
  z[234] = (z[155]*z[231]-z[159]*z[233])/pow(z[155],2);
  z[235] = z[230] - z[162]*z[226] - z[234]*z[157];
  z[236] = -z[126]*z[229] - M3*z[45]*z[46]*z[158];
  z[237] = z[236] - z[156]*z[234] - z[162]*z[228];
  z[238] = (z[163]*z[235]-z[237]*z[164])/pow(z[163],2);
  z[239] = (z[233]*(z[157]-z[156]*z[165])-z[155]*(z[226]-z[156]*z[238]-z[228]*
  z[165]))/pow(z[155],2);
  z[240] = (z[223]*(z[148]-z[125]*z[166]-z[126]*z[165])+z[124]*(z[221]+z[126]*
  z[238]+z[222]*z[166]-z[125]*z[239]-M3*z[45]*z[46]*z[165]))/pow(z[124],2);
  z[241] = cosh(BETA*U1);
  z[242] = BETA*C1;
  z[243] = V1 + z[242]/pow(z[241],2);
  z[244] = z[34]*(z[16]*U1+z[22]);
  z[245] = z[4]*z[244] - z[16]*(z[35]*U1+z[36]*U2) - z[35]*(z[22]+z[23]);
  z[246] = z[6]*z[245] - z[5]*z[7]*z[244];
  z[247] = z[246] - z[16]*(z[37]*U1+z[38]*U2+z[46]*U3) - z[37]*(z[22]+z[23]+
  z[24]);
  z[248] = z[7]*z[245] + z[5]*z[6]*z[244];
  z[249] = z[35]*(z[16]*U1+z[22]);
  z[250] = z[16]*(z[35]*U1+z[36]*U2) + z[35]*(z[22]+z[23]);
  z[251] = z[16]*(z[37]*U1+z[38]*U2) + z[37]*(z[22]+z[23]);
  z[252] = M3*(z[45]*z[247]+z[47]*z[248]) + 0.5*M1*z[5]*(z[34]*z[249]-z[35]*
  z[244]) + 0.5*M2*(z[7]*z[37]*z[250]+2*z[5]*z[34]*z[250]-2*z[5]*z[35]*z[244]-
  z[7]*z[35]*z[251]-z[34]*z[120]*z[251]-z[37]*z[119]*z[244]);
  z[253] = M3*(z[44]*z[247]+z[48]*z[248]) + 0.5*M2*(z[7]*z[38]*z[250]-2*z[5]*
  z[36]*z[244]-z[7]*z[36]*z[251]-z[38]*z[119]*z[244]);
  z[254] = z[253] - z[154]*(z[252]+z[16]*z[243]);
  z[255] = -M3*z[46]*z[248] - z[158]*(z[252]+z[16]*z[243]);
  z[256] = (z[255]-z[162]*z[254])/z[163];
  z[257] = (z[254]-z[156]*z[256])/z[155];
  z[258] = (z[252]+z[16]*z[243]+z[125]*z[257]+z[126]*z[256])/z[124];
  z[259] = -z[17]*(z[35]*U1+z[36]*U2) - z[36]*(z[22]+z[23]);
  z[260] = z[6]*z[259] - z[17]*(z[37]*U1+z[38]*U2+z[46]*U3) - z[38]*(z[22]+
  z[23]+z[24]);
  z[261] = z[17]*(z[35]*U1+z[36]*U2) + z[36]*(z[22]+z[23]);
  z[262] = z[17]*(z[37]*U1+z[38]*U2) + z[38]*(z[22]+z[23]);
  z[263] = M3*(z[45]*z[260]+z[7]*z[47]*z[259]) + 0.5*M2*(z[7]*z[37]*z[261]+2*
  z[5]*z[34]*z[261]-z[7]*z[35]*z[262]-z[34]*z[120]*z[262]);
  z[264] = cosh(BETA*U2);
  z[265] = BETA*C2;
  z[266] = V2 + z[265]/pow(z[264],2);
  z[267] = M3*(z[44]*z[260]+z[7]*z[48]*z[259]) - 0.5*M2*z[7]*(z[36]*z[262]-
  z[38]*z[261]);
  z[268] = z[267] + z[17]*z[266] - z[154]*z[263];
  z[269] = M3*z[7]*z[46]*z[259];
  z[270] = -z[269] - z[158]*z[263] - z[162]*z[268];
  z[271] = (z[268]-z[156]*z[270]/z[163])/z[155];
  z[272] = (z[263]+z[125]*z[271]+z[126]*z[270]/z[163])/z[124];
  z[273] = -z[18]*(z[37]*U1+z[38]*U2+z[46]*U3) - z[46]*(z[22]+z[23]+z[24]);
  z[274] = M3*(z[44]-z[45]*z[154])*z[273];
  z[275] = cosh(BETA*U3);
  z[276] = BETA*C3;
  z[277] = V3 + z[276]/pow(z[275],2);
  z[278] = z[18]*z[277] - M3*z[45]*z[158]*z[273];
  z[279] = (z[278]-z[162]*z[274])/z[163];
  z[280] = (z[274]-z[156]*z[279])/z[155];
  z[281] = (z[125]*z[280]+z[126]*z[279]+M3*z[45]*z[273])/z[124];
  z[282] = z[49]*(z[10]*z[12]-z[11]*z[13]);
  z[283] = z[49]*(z[10]*z[13]+z[11]*z[12]);
  z[284] = z[10]*z[12] - z[11]*z[13];
  z[285] = 0.5*M1*z[10]*(z[49]*z[75]-z[50]*z[74]) - 0.5*M2*(2*z[10]*z[50]*
  z[74]-2*z[10]*z[49]*z[76]-z[49]*z[284]*z[77]-z[52]*z[121]*z[74]) - M3*(
  z[60]*z[123]*z[74]+z[62]*z[121]*z[74]-z[282]*z[83]-z[12]*z[282]*z[82]-z[13]*
  z[283]*z[82]-z[121]*z[282]*z[74]-z[122]*z[283]*z[74]);
  z[286] = 0.5*M2*z[49]*(z[53]*z[123]+2*z[11]*z[51]) + M3*(z[53]*z[283]+z[12]*
  z[51]*z[283]-z[13]*z[51]*z[282]);
  z[287] = z[49]*(2*M3*(z[59]*z[284]+z[63]*z[123])-M2*(z[53]*z[123]+2*z[11]*
  z[51]));
  z[288] = M1*z[11]*z[49]*z[50] + M2*z[49]*(z[52]*z[123]+2*z[11]*z[50]) - M3*(
  z[13]*z[50]*z[282]+z[49]*z[60]*z[284]+z[49]*z[62]*z[123]-z[52]*z[283]-z[12]*
  z[50]*z[283]-z[49]*z[121]*z[283]-z[49]*z[123]*z[282]);
  z[289] = (z[136]*z[287]+2*z[140]*z[288])/pow(z[136],2);
  z[290] = (2*M3*(z[59]*z[123]+z[63]*z[121])+M2*(2*z[10]*z[51]-z[53]*z[121]))*
  z[74];
  z[291] = -0.5*z[290] - z[168]*z[285] - 0.5*z[289]*z[151];
  z[292] = M3*(z[61]*z[283]+z[12]*z[81]*z[283]-z[13]*z[81]*z[282]);
  z[293] = z[168]*z[292] - 0.5*z[138]*z[289];
  z[294] = M3*z[49]*z[61]*z[123];
  z[295] = (z[136]*z[294]-z[144]*z[288])/pow(z[136],2);
  z[296] = M3*z[61]*z[121]*z[74];
  z[297] = z[296] + z[295]*z[151] - z[172]*z[285];
  z[298] = z[137]*z[295] + z[172]*z[286];
  z[299] = z[168]*z[286] - 0.5*z[137]*z[289];
  z[300] = (z[169]*z[298]-z[173]*z[299])/pow(z[169],2);
  z[301] = z[297] - z[176]*z[291] - z[300]*z[171];
  z[302] = z[138]*z[295] + z[172]*z[292];
  z[303] = z[302] - z[170]*z[300] - z[176]*z[293];
  z[304] = (z[177]*z[301]-z[303]*z[178])/pow(z[177],2);
  z[305] = (z[299]*(z[171]-z[170]*z[179])-z[169]*(z[291]-z[170]*z[304]-z[293]*
  z[179]))/pow(z[169],2);
  z[306] = (z[288]*(z[151]-z[137]*z[180]-z[138]*z[179])-z[136]*(z[137]*z[305]-
  z[285]-z[138]*z[304]-z[286]*z[180]-z[292]*z[179]))/pow(z[136],2);
  z[307] = z[13]*z[55] - z[12]*z[56];
  z[308] = -0.5*M2*(z[12]*z[50]*z[77]-z[12]*z[52]*z[76]-z[49]*z[284]*z[77]-
  z[52]*z[121]*z[74]) - M3*(z[307]*z[83]+z[12]*z[62]*z[82]+z[12]*z[307]*z[82]+
  z[60]*z[122]*z[74]+z[60]*z[123]*z[74]+z[62]*z[121]*z[74]+z[121]*z[307]*
  z[74]);
  z[309] = -M3*(z[53]*z[60]-z[13]*z[51]*z[62]-z[13]*z[51]*z[307]) - 0.5*M2*(
  z[13]*z[50]*z[53]+z[13]*z[51]*z[52]-z[49]*z[53]*z[123]);
  z[310] = M3*(z[12]*z[50]*z[59]+z[13]*z[50]*z[63]-z[13]*z[51]*z[52]-z[49]*
  z[59]*z[284]-z[49]*z[63]*z[123]-z[12]*z[49]*z[51]*z[123]-z[13]*z[49]*z[51]*
  z[121]) - 0.5*M2*(z[13]*z[50]*z[53]+z[13]*z[51]*z[52]-z[49]*z[53]*z[123]);
  z[311] = M3*(z[13]*z[50]*z[62]+z[13]*z[50]*z[307]-z[52]*z[60]-z[49]*z[60]*
  z[121]-z[49]*z[60]*z[284]-z[49]*z[62]*z[123]-z[49]*z[123]*z[307]) - M2*
  z[52]*(z[13]*z[50]-z[49]*z[123]);
  z[312] = (z[136]*z[310]-z[140]*z[311])/pow(z[136],2);
  z[313] = M3*(z[13]*z[59]*z[82]-z[12]*z[51]*z[83]-z[12]*z[63]*z[82]-z[59]*
  z[123]*z[74]-z[63]*z[121]*z[74]-z[12]*z[51]*z[121]*z[74]-z[13]*z[51]*z[122]*
  z[74]-z[51]*pow(z[12],2)*z[82]-z[51]*pow(z[13],2)*z[82]) - 0.5*M2*(z[12]*
  z[51]*z[77]-z[12]*z[53]*z[76]-z[53]*z[121]*z[74]);
  z[314] = z[313] + z[312]*z[151] - z[168]*z[308];
  z[315] = M3*(z[60]*z[61]-z[13]*z[62]*z[81]-z[13]*z[81]*z[307]);
  z[316] = M3*(z[13]*z[51]*z[61]-z[12]*z[59]*z[81]-z[13]*z[63]*z[81]);
  z[317] = z[316] + z[138]*z[312] - z[168]*z[315];
  z[318] = M3*z[61]*(z[13]*z[50]-z[49]*z[123]);
  z[319] = (z[136]*z[318]+z[144]*z[311])/pow(z[136],2);
  z[320] = M3*z[61]*(z[12]*z[82]+z[121]*z[74]);
  z[321] = z[320] - z[172]*z[308] - z[319]*z[151];
  z[322] = M3*z[13]*z[51]*z[61];
  z[323] = z[322] + z[172]*z[309] - z[137]*z[319];
  z[324] = z[51]*(M2*z[13]*z[53]+M3*(z[13]*z[53]-z[12]*z[59]-z[13]*z[63]));
  z[325] = z[324] + z[137]*z[312] + z[168]*z[309];
  z[326] = (z[169]*z[323]-z[173]*z[325])/pow(z[169],2);
  z[327] = z[321] - z[176]*z[314] - z[326]*z[171];
  z[328] = M3*z[13]*z[61]*z[81];
  z[329] = z[328] - z[138]*z[319] - z[172]*z[315];
  z[330] = z[329] - z[170]*z[326] - z[176]*z[317];
  z[331] = (z[177]*z[327]-z[330]*z[178])/pow(z[177],2);
  z[332] = (z[325]*(z[171]-z[170]*z[179])-z[169]*(z[314]-z[170]*z[331]-z[317]*
  z[179]))/pow(z[169],2);
  z[333] = (z[311]*(z[151]-z[137]*z[180]-z[138]*z[179])+z[136]*(z[308]+z[138]*
  z[331]+z[309]*z[180]-z[137]*z[332]-z[315]*z[179]))/pow(z[136],2);
  z[334] = cosh(BETA*U4);
  z[335] = BETA*C4;
  z[336] = V4 + z[335]/pow(z[334],2);
  z[337] = z[50]*(z[19]*U4+z[28]);
  z[338] = z[49]*(z[19]*U4+z[28]);
  z[339] = z[19]*(z[52]*U4+z[53]*U5+z[61]*U6) + z[52]*(z[28]+z[29]+z[30]);
  z[340] = z[19]*(z[50]*U4+z[51]*U5+z[81]*U6) + z[50]*(z[28]+z[29]+z[30]);
  z[341] = z[19]*(z[52]*U4+z[53]*U5) + z[52]*(z[28]+z[29]);
  z[342] = z[19]*(z[50]*U4+z[51]*U5) + z[50]*(z[28]+z[29]);
  z[343] = 0.5*M1*z[11]*(z[49]*z[337]-z[50]*z[338]) - M3*(z[60]*z[339]+z[12]*
  z[60]*z[340]+z[13]*z[62]*z[340]+z[60]*z[121]*z[338]+z[62]*z[122]*z[338]) - 
  0.5*M2*(z[13]*z[50]*z[341]+2*z[11]*z[50]*z[338]-2*z[11]*z[49]*z[342]-z[13]*
  z[52]*z[342]-z[49]*z[123]*z[341]-z[52]*z[122]*z[338]);
  z[344] = -M3*(z[59]*z[339]+z[12]*z[59]*z[340]+z[13]*z[63]*z[340]+z[59]*
  z[121]*z[338]+z[63]*z[122]*z[338]) - 0.5*M2*(z[13]*z[51]*z[341]+2*z[11]*
  z[51]*z[338]-z[13]*z[53]*z[342]-z[53]*z[122]*z[338]);
  z[345] = z[344] - z[168]*(z[343]+z[19]*z[336]);
  z[346] = M3*z[61]*(z[13]*z[340]+z[122]*z[338]);
  z[347] = z[346] - z[172]*(z[343]+z[19]*z[336]);
  z[348] = (z[347]-z[176]*z[345])/z[177];
  z[349] = (z[345]-z[170]*z[348])/z[169];
  z[350] = (z[343]+z[19]*z[336]+z[137]*z[349]+z[138]*z[348])/z[136];
  z[351] = z[20]*(z[52]*U4+z[53]*U5+z[61]*U6) + z[53]*(z[28]+z[29]+z[30]);
  z[352] = z[20]*(z[50]*U4+z[51]*U5+z[81]*U6) + z[51]*(z[28]+z[29]+z[30]);
  z[353] = z[20]*(z[52]*U4+z[53]*U5) + z[53]*(z[28]+z[29]);
  z[354] = z[20]*(z[50]*U4+z[51]*U5) + z[51]*(z[28]+z[29]);
  z[355] = -M3*(z[60]*z[351]+z[12]*z[60]*z[352]+z[13]*z[62]*z[352]) - 0.5*M2*(
  z[13]*z[50]*z[353]-2*z[11]*z[49]*z[354]-z[13]*z[52]*z[354]-z[49]*z[123]*
  z[353]);
  z[356] = cosh(BETA*U5);
  z[357] = BETA*C5;
  z[358] = V5 + z[357]/pow(z[356],2);
  z[359] = -0.5*M2*z[13]*(z[51]*z[353]-z[53]*z[354]) - M3*(z[59]*z[351]+z[12]*
  z[59]*z[352]+z[13]*z[63]*z[352]);
  z[360] = z[359] + z[20]*z[358] - z[168]*z[355];
  z[361] = M3*z[13]*z[61]*z[352];
  z[362] = z[361] - z[172]*z[355] - z[176]*z[360];
  z[363] = (z[360]-z[170]*z[362]/z[177])/z[169];
  z[364] = (z[355]+z[137]*z[363]+z[138]*z[362]/z[177])/z[136];
  z[365] = z[21]*(z[52]*U4+z[53]*U5+z[61]*U6) + z[61]*(z[28]+z[29]+z[30]);
  z[366] = z[21]*(z[50]*U4+z[51]*U5+z[81]*U6) + z[81]*(z[28]+z[29]+z[30]);
  z[367] = M3*(z[60]*z[365]+z[12]*z[60]*z[366]+z[13]*z[62]*z[366]);
  z[368] = M3*(z[59]*z[365]+z[12]*z[59]*z[366]+z[13]*z[63]*z[366]);
  z[369] = cosh(BETA*U6);
  z[370] = BETA*C6;
  z[371] = V6 + z[370]/pow(z[369],2);
  z[372] = M3*z[13]*z[61]*z[366];
  z[373] = z[372] + z[21]*z[371] + z[172]*z[367];
  z[374] = z[373] + z[176]*(z[368]-z[168]*z[367]);
  z[375] = (z[168]*z[367]-z[368]-z[170]*z[374]/z[177])/z[169];
  z[376] = (z[367]-z[137]*z[375]-z[138]*z[374]/z[177])/z[136];
  
  DFDU[0][0] = 0;
  DFDU[0][1] = 0;
  DFDU[0][2] = 0;
  DFDU[0][3] = 0;
  DFDU[0][4] = 0;
  DFDU[0][5] = 0;
  DFDU[1][0] = 0;
  DFDU[1][1] = 0;
  DFDU[1][2] = 0;
  DFDU[1][3] = 0;
  DFDU[1][4] = 0;
  DFDU[1][5] = 0;
  DFDU[2][0] = 0;
  DFDU[2][1] = 0;
  DFDU[2][2] = 0;
  DFDU[2][3] = 0;
  DFDU[2][4] = 0;
  DFDU[2][5] = 0;
  DFDU[3][0] = 0;
  DFDU[3][1] = 0;
  DFDU[3][2] = 0;
  DFDU[3][3] = 0;
  DFDU[3][4] = 0;
  DFDU[3][5] = 0;
  DFDU[4][0] = 0;
  DFDU[4][1] = 0;
  DFDU[4][2] = 0;
  DFDU[4][3] = 0;
  DFDU[4][4] = 0;
  DFDU[4][5] = 0;
  DFDU[5][0] = 0;
  DFDU[5][1] = 0;
  DFDU[5][2] = 0;
  DFDU[5][3] = 0;
  DFDU[5][4] = 0;
  DFDU[5][5] = 0;
  DFDU[6][0] = z[184];
  DFDU[6][1] = z[186];
  DFDU[6][2] = -z[188];
  DFDU[6][3] = 0;
  DFDU[6][4] = 0;
  DFDU[6][5] = 0;
  DFDU[7][0] = z[183];
  DFDU[7][1] = -z[185];
  DFDU[7][2] = z[187];
  DFDU[7][3] = 0;
  DFDU[7][4] = 0;
  DFDU[7][5] = 0;
  DFDU[8][0] = z[182]/z[163];
  DFDU[8][1] = z[17]*z[162]/z[163];
  DFDU[8][2] = -z[18]/z[163];
  DFDU[8][3] = 0;
  DFDU[8][4] = 0;
  DFDU[8][5] = 0;
  DFDU[9][0] = 0;
  DFDU[9][1] = 0;
  DFDU[9][2] = 0;
  DFDU[9][3] = z[191];
  DFDU[9][4] = z[193];
  DFDU[9][5] = -z[195];
  DFDU[10][0] = 0;
  DFDU[10][1] = 0;
  DFDU[10][2] = 0;
  DFDU[10][3] = z[190];
  DFDU[10][4] = -z[192];
  DFDU[10][5] = z[194];
  DFDU[11][0] = 0;
  DFDU[11][1] = 0;
  DFDU[11][2] = 0;
  DFDU[11][3] = z[189]/z[177];
  DFDU[11][4] = z[20]*z[176]/z[177];
  DFDU[11][5] = -z[21]/z[177];
  DFDX[0][0] = 0;
  DFDX[0][1] = 0;
  DFDX[0][2] = 0;
  DFDX[0][3] = 0;
  DFDX[0][4] = 0;
  DFDX[0][5] = 0;
  DFDX[0][6] = 1;
  DFDX[0][7] = 0;
  DFDX[0][8] = 0;
  DFDX[0][9] = 0;
  DFDX[0][10] = 0;
  DFDX[0][11] = 0;
  DFDX[1][0] = 0;
  DFDX[1][1] = 0;
  DFDX[1][2] = 0;
  DFDX[1][3] = 0;
  DFDX[1][4] = 0;
  DFDX[1][5] = 0;
  DFDX[1][6] = 0;
  DFDX[1][7] = 1;
  DFDX[1][8] = 0;
  DFDX[1][9] = 0;
  DFDX[1][10] = 0;
  DFDX[1][11] = 0;
  DFDX[2][0] = 0;
  DFDX[2][1] = 0;
  DFDX[2][2] = 0;
  DFDX[2][3] = 0;
  DFDX[2][4] = 0;
  DFDX[2][5] = 0;
  DFDX[2][6] = 0;
  DFDX[2][7] = 0;
  DFDX[2][8] = 1;
  DFDX[2][9] = 0;
  DFDX[2][10] = 0;
  DFDX[2][11] = 0;
  DFDX[3][0] = 0;
  DFDX[3][1] = 0;
  DFDX[3][2] = 0;
  DFDX[3][3] = 0;
  DFDX[3][4] = 0;
  DFDX[3][5] = 0;
  DFDX[3][6] = 0;
  DFDX[3][7] = 0;
  DFDX[3][8] = 0;
  DFDX[3][9] = 1;
  DFDX[3][10] = 0;
  DFDX[3][11] = 0;
  DFDX[4][0] = 0;
  DFDX[4][1] = 0;
  DFDX[4][2] = 0;
  DFDX[4][3] = 0;
  DFDX[4][4] = 0;
  DFDX[4][5] = 0;
  DFDX[4][6] = 0;
  DFDX[4][7] = 0;
  DFDX[4][8] = 0;
  DFDX[4][9] = 0;
  DFDX[4][10] = 1;
  DFDX[4][11] = 0;
  DFDX[5][0] = 0;
  DFDX[5][1] = 0;
  DFDX[5][2] = 0;
  DFDX[5][3] = 0;
  DFDX[5][4] = 0;
  DFDX[5][5] = 0;
  DFDX[5][6] = 0;
  DFDX[5][7] = 0;
  DFDX[5][8] = 0;
  DFDX[5][9] = 0;
  DFDX[5][10] = 0;
  DFDX[5][11] = 1;
  DFDX[6][0] = -z[218];
  DFDX[6][1] = -z[240];
  DFDX[6][2] = 0;
  DFDX[6][3] = 0;
  DFDX[6][4] = 0;
  DFDX[6][5] = 0;
  DFDX[6][6] = -z[258];
  DFDX[6][7] = -z[272];
  DFDX[6][8] = -z[281];
  DFDX[6][9] = 0;
  DFDX[6][10] = 0;
  DFDX[6][11] = 0;
  DFDX[7][0] = -z[217];
  DFDX[7][1] = -z[239];
  DFDX[7][2] = 0;
  DFDX[7][3] = 0;
  DFDX[7][4] = 0;
  DFDX[7][5] = 0;
  DFDX[7][6] = z[257];
  DFDX[7][7] = z[271];
  DFDX[7][8] = z[280];
  DFDX[7][9] = 0;
  DFDX[7][10] = 0;
  DFDX[7][11] = 0;
  DFDX[8][0] = z[216];
  DFDX[8][1] = z[238];
  DFDX[8][2] = 0;
  DFDX[8][3] = 0;
  DFDX[8][4] = 0;
  DFDX[8][5] = 0;
  DFDX[8][6] = z[256];
  DFDX[8][7] = z[270]/z[163];
  DFDX[8][8] = z[279];
  DFDX[8][9] = 0;
  DFDX[8][10] = 0;
  DFDX[8][11] = 0;
  DFDX[9][0] = 0;
  DFDX[9][1] = 0;
  DFDX[9][2] = 0;
  DFDX[9][3] = -z[306];
  DFDX[9][4] = -z[333];
  DFDX[9][5] = 0;
  DFDX[9][6] = 0;
  DFDX[9][7] = 0;
  DFDX[9][8] = 0;
  DFDX[9][9] = -z[350];
  DFDX[9][10] = -z[364];
  DFDX[9][11] = z[376];
  DFDX[10][0] = 0;
  DFDX[10][1] = 0;
  DFDX[10][2] = 0;
  DFDX[10][3] = -z[305];
  DFDX[10][4] = -z[332];
  DFDX[10][5] = 0;
  DFDX[10][6] = 0;
  DFDX[10][7] = 0;
  DFDX[10][8] = 0;
  DFDX[10][9] = z[349];
  DFDX[10][10] = z[363];
  DFDX[10][11] = z[375];
  DFDX[11][0] = 0;
  DFDX[11][1] = 0;
  DFDX[11][2] = 0;
  DFDX[11][3] = z[304];
  DFDX[11][4] = z[331];
  DFDX[11][5] = 0;
  DFDX[11][6] = 0;
  DFDX[11][7] = 0;
  DFDX[11][8] = 0;
  DFDX[11][9] = z[348];
  DFDX[11][10] = z[362]/z[177];
  DFDX[11][11] = z[374]/z[177];
}

